/*
KM_base 的油门和转向都使用PPM占空比控制
*/

# pragma once

# include "cpp_tools.h"
# include "udpSettings.h"
# include "chassis_controller_base.h"

# ifdef OUTPUT_REAL
# include <wiringPi.h>
# endif // OUTPUT_REAL

namespace pwm
{
    namespace steering
    {
        int pin = 12;
        double pwm_add = 100./3.; // 根据实际转向角度调整
        double pwm_0 = 150; // 根据实际转向角度调整
        double pwm_minus = 100./3.; // 根据实际转向角度调整
    } // namespace steering
    
    namespace drive
    {
        int pin = 18;
        double pwm_max = 200;
        double pwm_mid = 150;
        double pwm_min = 100;
    } // namespace drive
};

class KM_base : public chassis_controller
{
protected: 
    cpp_tools::mtDataManager<controlOutPut> output;
    
    const double steering_update = 100; // 转向pwm输出量的更新频率
    const double drive_update = 100; // 驱动pwm输出量的更新频率

    const double pi = M_PI;

    cpp_tools::threadPool m_threadPool;
    
    bool isRunning = true;

    int steeringOption = 2; // 选择映射方法

    // ==================================================================
    virtual double angle2delta(const double angle) const
    {
        static const double la = 95 - 9.8;
        static const double lb = 50 - 5;
        static const double lc = 25;
        static const double ld = 79;
    
        static const double lu = 23;
    
        const double l1 = la - lu*sin(angle);
        const double l2 = lb - lu*cos(angle);
        const double l = std::sqrt(std::pow(l1,2) + std::pow(l2,2));
    
        const double theta = std::acos((std::pow(lc,2) + std::pow(l,2) - std::pow(ld,2))/(2*l*lc));
        const double phi = std::atan2(l2,l1);
    
        return pi/2 - theta - phi - 0.2518932484737138;
    }

    virtual void outPutSteering(double steering)
    {
        // std::cout << "输出转角：" << steering << std::endl;

        # ifndef OUTPUT_REAL

        static cpp_tools::udpVecSender<double> steeringSender(udpSettings::IP::sim,udpSettings::PORT::lower_sim::steering,1);

        steeringSender.send(Eigen::VectorXd::Constant(1,steering));

        # else // OUTPUT_REAL

        double pulseWidth;
        steering = cpp_tools::saturation(steering,-1,1);
        if (steering >= 0)
        {
            pulseWidth = -steering*pwm::steering::pwm_add + pwm::steering::pwm_0;
        }
        else
        {
            pulseWidth = -steering*pwm::steering::pwm_minus + pwm::steering::pwm_0;
        }

        pwm_write(pwm::steering::pin,pulseWidth);

        # endif // OUTPUT_REAL
    }

    virtual void outPutDrive(double drive)
    {
        // std::cout << "输出油门：" << drive << std::endl;

        # ifndef OUTPUT_REAL
        
        static cpp_tools::udpVecSender<double> driveSender(udpSettings::IP::sim,udpSettings::PORT::lower_sim::drive,1);

        driveSender.send(Eigen::VectorXd::Constant(1,drive));

        # else // OUTPUT_REAL
        
        double pulseWidth;
        drive = cpp_tools::saturation(drive,-1,1);
        if (drive >= 0)
        {
            pulseWidth = drive*(pwm::drive::pwm_max - pwm::drive::pwm_mid) + pwm::drive::pwm_mid;
        }
        else
        {
            pulseWidth = drive*(pwm::drive::pwm_mid - pwm::drive::pwm_min) + pwm::drive::pwm_mid;
        }

        pwm_write(pwm::drive::pin,pulseWidth);

        # endif // OUTPUT_REAL
    }

    virtual void pwm_init()
    {
        # ifdef OUTPUT_REAL
        wiringPiSetupGpio(); // 这个似乎多线程不安全，所以只在最开始执行一次
        pwmSetMode(PWM_MODE_MS);
        pwmSetRange(1000);
        pwmSetClock(192);
        # endif
    }

    virtual void pwm_clear()
    {
        # ifdef OUTPUT_REAL
        pinMode(pwm::steering::pin, INPUT);
        pinMode(pwm::drive::pin, INPUT);
        # endif
    }

    virtual void pwm_write(int pin, double pulseWidth)
    {
        # ifdef OUTPUT_REAL
        pwmWrite(pin, pulseWidth);
        # endif
    }

    virtual void pwm_pinMode(int pin)
    {
        # ifdef OUTPUT_REAL
        pinMode(pwm::steering::pin,PWM_MS_OUTPUT);
        # endif
    }

public:
    /// @brief 连接手柄输入和UDP输入并控制RC底盘的转向和油门/刹车(PWM输入)
    KM_base()
    {
        std::cout << "KM base start" << std::endl;

        pwm_init();

        // ==========================循环输出转角============================
        std::thread t_steering([&]() -> void {
            pwm_pinMode(pwm::steering::pin);

            // 记录
            size_t steering_files = 0;
            cpp_tools::csvRecorder steeringRecorder(host_path + data_path + "/control/steering.csv");
            
            double steering_last = -9999;
            while (isRunning)
            {
                auto t = std::chrono::steady_clock::now();

                double steering = output.read().access()->steering;

                // 输出
                outPutSteering(steering);

                if (steering != steering_last)
                {
                    steering_last = steering;

                    // 保存
                    Eigen::VectorXd data(2);
                    data << cpp_tools::getCurrentTimeAsDouble(),steering;
                    steeringRecorder.record(data);
                }

                std::this_thread::sleep_until(t + std::chrono::microseconds(std::max((int)(1e6/steering_update),1)));
            }
        });
        m_threadPool.push_back(t_steering);

        // ==========================循环输出驱动力============================
        std::thread t_drive([&]() -> void {
            // 原则：
            //  1.若手柄连接超时则全部输出0
            //  2.当且仅当手柄的D_paid上方被按下时(即D_paid_fr = 1)使用UDP收到的控制信号，否则使用手柄的控制信号

            pwm_pinMode(pwm::drive::pin);

            // 记录
            size_t drive_files = 0;
            cpp_tools::csvRecorder driveRecorder(host_path + data_path + "/control/drive.csv");
            
            double drive_last = -9999;
            while (isRunning)
            {
                auto t = std::chrono::steady_clock::now();

                double drive = output.read().access()->drive;

                // 输出
                outPutDrive(drive);

                if (drive != drive_last)
                {
                    drive_last = drive;

                    // 保存
                    Eigen::VectorXd data(2);
                    data << cpp_tools::getCurrentTimeAsDouble(),drive;
                    driveRecorder.record(data);
                }
                
                std::this_thread::sleep_until(t + std::chrono::microseconds(std::max((int)(1e6/drive_update),1)));
            }
        });
        m_threadPool.push_back(t_drive);
    };

    ~KM_base()
    {
        shutdown();

        pwm_clear();

        std::cout << "output controller exit." << std::endl;
    };

    void shutdown() override
    {
        isRunning = false;

        m_threadPool.joinAll();
    }

    /// @brief 设置输出值
    /// @param steering 转向 [-1, 1]
    /// @param drive 驱动 [-1, 1]
    void setOutPut(double steering, double drive) override
    {
        output.write().access()->steering = steering;
        output.write().access()->drive = drive;
    }

    /// @brief 获取实际的输出值
    /// @return steering,drive
    controlOutPut getOutPut() override
    {
        return *output.read().access();
    }

    /// @brief 根据标定将期望的纵向力映射为油门量
    /// @param v 当前速度 m/s
    /// @param F_x_des 期望纵向力 N
    /// @return 油门控制量[-1,1]
    double forceMapping(const double v, const double F_x_des) const override
    {
        double u = 0;
    
        const double mass = 5.2; // 标定时的整车质量
        double a_des = F_x_des/mass; // 等效到标定时的加速度
    
        // 低速摩擦力补偿：
        //      由于低速线性差并且在低速情况下车辆横摆运动以及质心侧扁角动力学模型对于速度变化更加敏感，
        //      因此对摩擦力进行补偿以保证正常启动以及减少速度误差
        double compensition_a_base = 0.5; // 基础补偿加速度
        double compensition_a_dc = 1; // 最大补偿加速度
        double compensition_range_lbv = 3; // 补偿递减区间起点
        double compensition_range_ubv = 5; // 补偿递减区间终点
        if (F_x_des > 0)
        {
            a_des += compensition_a_base;
        }
        if (std::abs(v) <= compensition_range_lbv && F_x_des != 0)
        {
            a_des += compensition_a_dc;
        }
        else if (std::abs(v) <= compensition_range_ubv && F_x_des != 0)
        {
            a_des += (1 - (v - compensition_range_lbv)/(compensition_range_ubv - compensition_range_lbv))*compensition_a_dc;
        }
    
        // 根据a_des计算需要输出的油门指令u，a_des 正好等于0的情况下 u = 0
        if (a_des > 0)
        {
            const double p1 = -1.625*v + 30.1;
            const double p2 = -0.2403*v - 2.164;
    
            u = std::max((a_des - p2)/p1,0.);
        }
        else if (a_des < 0)
        {
            const double p1 = 5.023*v;
            const double p2 = 0.3972*v;
    
            u = std::min((a_des - p2)/p1,0.);
        }
    
        return cpp_tools::saturation(u,-1,1);
    };
    
    /// @brief 从期望前轮转角映射到RC控制的转向量
    /// @param delta_f_des 期望前轮转角, 单位 rad
    /// @return 返回范围为[-1,1]的转向行程 
    double steeringMapping(const double delta_f_des) const override
    {    
        double u;
        if (steeringOption == 1)
        {
            // ===============方法1，根据转向系统几何===============
            auto [sol,val,iter] = cpp_tools::goldenSection([&](double u) -> double {
                double angle = u*pi/6;
                double delta = cpp_tools::sign(u)*0.5*(std::abs(angle2delta(angle)) + std::abs(angle2delta(-angle)));
                return std::abs(delta_f_des - delta);
            },
            -1,1,1e-6,1e-6,50);
    
            u = sol;
        }
        else if (steeringOption == 2)
        {
            // ===============方法2，更加简单===============
            const double delta_f_max = 0.4;
            u = delta_f_des/delta_f_max;
        }
    
        return cpp_tools::saturation(u,-1,1);
    };

    KM_base(const KM_base&) = delete;
};
